/**

This file is part of MaCI/GIMnet.

MaCI/GIMnet is free software: you can redistribute it and/or modify it 
under the terms of the GNU Lesser General Public License as published 
by the Free Software Foundation, either version 3 of the License, or 
(at your option) any later version.

MaCI/GIMnet is distributed in the hope that it will be useful, but WITHOUT 
ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or 
FITNESS FOR A PARTICULAR PURPOSE.  See the GNU Lesser General Public 
License for more details.

You should have received a copy of the GNU Lesser General Public 
License along with GIMnet. (See COPYING.LESSER) If not, see 
<http://www.gnu.org/licenses/>.

**/
#ifndef A635SENSORS_H
#define A635SENSORS_H

#include "Object.h"
#include "Sick.h"
#include "Shapes.h"
//#include "MLMA.h"
#include "../MaCI/interfaces/SpeedCtrl/SpeedCtrlServer.hpp"
#include "../MaCI/interfaces/SpeedCtrl/SpeedCtrlTypes.hpp"
#include "../MaCI/interfaces/Position/PositionServer.hpp"
#include "../MaCI/interfaces/Position/PositionTypes.hpp"
#include "../MaCI/interfaces/Energy/EnergyServer.hpp"
#include <string>

#ifdef MOTTI_MAP_DECODER_SUPPORT
#include "MottiMap.hpp"
#endif


#include "GLProvider.h"



#define CHASSIS_WIDTH 1.00
#define CHASSIS_HEIGHT 0.10
#define CHASSIS_LENGTH 2.0

#define FORK_LENGTH 0.01
#define FORK_WIDTH 0.15
#define FORK_HEIGHT 0.05
#define FORK_GAP 0.6

#define GROUND_CLEARANCE 0.05

#define MAX_MOTOR_SPEED 1.0
#define MAX_ANGULAR_SPEED 1.0
#define MAX_ACCELERATION 0.2

#define POSITION_INTERVAL 0.1
#define ENERGY_INTERVAL 1.0


///NAVIGATION SCANNER
#define SCANNER_OFFSET_Y 1.0
#define SCANNER_OFFSET_Z 1.95
#define SCANNER_OFFSET_X 0

#define SCANNER_YAW M_PI
#define SCANNER_ROLL 0
#define SCANNER_PITCH 0

#define LASER_INTERVAL 0.1

///REAR-tilted laser
#define REAR_SCANNER_OFFSET_Y 2.0
#define REAR_SCANNER_OFFSET_Z 1.95 ///Rungon suunnassa siirtymä (eteen -)
#define REAR_SCANNER_OFFSET_X 0

#define REAR_SCANNER_YAW M_PI
#define REAR_SCANNER_ROLL 0
#define REAR_SCANNER_PITCH 20*M_PI/180.0


///FRONT-tilted laser
#define FRONT_SCANNER_OFFSET_Y 2.0
#define FRONT_SCANNER_OFFSET_Z 0
#define FRONT_SCANNER_OFFSET_X 0

#define FRONT_SCANNER_YAW 0
#define FRONT_SCANNER_ROLL 0
#define FRONT_SCANNER_PITCH 20*M_PI/180.0

//class xmlNode;


struct TLaserPose3D{
	float x;
	float y;
	float z;
	float roll;
	float pitch;
	float yaw;
	std::string name;
	TLaserPose3D(){
		x=0;y=0;z=0;
		roll=0;pitch=0;yaw=0;
	}
	TLaserPose3D(float _x,float _y,float _z, float ro, float pi, float ya, std::string _name){
		x=_x;y=_y;z=_z;
		roll=ro;pitch=pi;yaw=ya;
		name = _name;
	}
	void set(float _x,float _y,float _z, float ro, float pi, float ya, std::string _name){
		x=_x;y=_y;z=_z;
		roll=ro;pitch=pi;yaw=ya;
		name = _name;
	}
};




class CA635Sensors : public CObject
{
  public:
    
    CA635Sensors(CWorld* World, float x, float y, float heading, std::string gimnetName, std::string MaCIGroup,
								 TLaserPose3D navi, TLaserPose3D rear, TLaserPose3D front);
    
    void Activate();

    void Update(float StepSize);

    void OnCollision(CPrimitive *primitive, int numc, dContact *contacts, CPrimitive *primitive2);
    
    void Draw();
    
    const float IdlePowerConsumption;
    
    const float DrivePowerConsumptionMultiplier;
    
    const float MaxBatteryLevel;
    
    static CObject* LoadMLMA(CWorld *world, xmlNode *node);
    
 private: // Variables
    gimi::GIMI gimi;
    MaCI::MaCICtrl::CMaCICtrlServer mcs;
    MaCI::SpeedCtrl::CSpeedCtrlServer SpeedCtrlServer;
    MaCI::Position::CPositionServer PositionServer;
    MaCI::Energy::CEnergyServer EnergyServer;
    
    std::string gimnetName;
    std::string MaCIGroup;
    float speed, angularSpeed, posX, posY, heading;
    float batteryLevel, powerConsumption;
    
    float positionTimer, energyTimer;
    
    CBox *chassis;
    CBox *leftFork, *rightFork;
    TMaterial chassisMaterial, forkMaterial;
    CSick *LaserScanner;
		CSick *rearLaserScanner;
		CSick *frontLaserScanner;
		TLaserPose3D lms291;
		TLaserPose3D lms111rear;
		TLaserPose3D lms111front;


    void renderStrokeFontString(
                                float x, 
                                float y, 
                                float z, 
                                void *font, 
                                const char *string);


    CWorld* iWorld;
    CGLProvider* iGLProvider;


#ifdef MOTTI_MAP_DECODER_SUPPORT
 public:
    void SetMottiMapFilename(const std::string &aMottiMapFilename);
 private:
    CMottiMap iMottiMap;
    std::string iMottiMapFilename;
#endif
    ownTime_ms_t iLastBatteryStatusUpdate;

 private: // Functions
    void updateBattery(const float &aStepSize);
    
    void connectGIMI();
    
};

#endif
